//
// Created by luo on 2022-10-17.
//

#include<opencv2/opencv.hpp>
#include<iostream>
#include<vector>
#include<algorithm>
#include"../include/Buff_detect.h"
#include"tool.h"
#include <opencv2/ml/ml.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/core.hpp>
//#include <opencv2/dnn.hpp>


//#define Predeict 1
#define NO_TARGET -1
//#define FloodFill 1
//#define Erode_RGB 1
//#define _minEnclosingCircle 1
using namespace cv;
using namespace std;
using namespace ml;
constexpr int advance_angle = 40;


buffdistinguish::buffdistinguish(){
    //Ptr<SVM> SVM_params = SVM::create();
    //SVM_params = SVM::load("buff_svm.xml");
}

//---------------------图像预处理------------------------//
void buffdistinguish::imagePreprocess(cv::Mat &frame, std::string &Owecolor) {

    this->frame_clone = frame.clone();
    cv::Mat reduce_src= frame.clone();
    //reduce_src(ROI_1);
    int Row = frame.rows;
    int Col = frame.cols;
    static cv::Mat ROI_1;
    m_img_rect = cv::Rect(cv::Point(0, 0), frame.size());

#if Erode_RGB
    //for (int i = 0; i < Row; i++)
	//{
	//	for (int j = 0; j < Col; j++)
	//	{
	//		frame.ptr<Vec3b>(i)[j][0] = frame.ptr<Vec3b>(i)[j][0] * 0.35;
	//	}
	//}
	//for (int i = 0; i < Row; i++)
	//{
	//	for (int j = 0; j < Col; j++)
	//	{
	//		frame.ptr<Vec3b>(i)[j][1] = frame.ptr<Vec3b>(i)[j][1] * 0.85;
	//	}
	//}
	//imshow("threshold", frame);

	/*if(target_buff_param.is_target_buff = true)
	{
		getROI(fan_param);
		m_roi_rect_last = m_img_rect + m_roi_rect_last.tl();
		m_roi_rect_last = m_roi_rect_last & m_img_rect;
		reduce_src == frame(m_roi_rect_last);
	}
	else
	{
		m_roi_rect_last = m_img_rect;
		reduce_src== frame;
	}*/
#endif
    Mat element1 = getStructuringElement(MORPH_RECT, Size(10, 10));//设置内核1
    Mat element2 = getStructuringElement(MORPH_RECT, Size(15, 15));//设置内核2
    Mat element3 = getStructuringElement(MORPH_RECT, Size(7, 7));//设置内核3
    Mat element4 = getStructuringElement(MORPH_RECT, Size(3, 3));//设置内核4
    Mat element5 = getStructuringElement(MORPH_RECT, Size(25, 25));//设置内核5
    Mat element6 = getStructuringElement(MORPH_RECT, Size(5, 5));//设置内核4
    //imshow("ROI_1", reduce_src);
    Mat src;
    cv::cvtColor(reduce_src, src, COLOR_BGR2GRAY);
    cv::GaussianBlur(src, src, cv::Size(3, 3), 0);        //去除高斯噪声
    cv::threshold(src, src, 55, 255, cv::THRESH_BINARY);  //阈值调低可以增大识别率
    //imshow("midImage 1", src);
    cv::dilate(src,src,15);
    std::vector<cv::Mat> imgChannels;
    split(frame, imgChannels);


    int structElementSize = 5;  //*****�������ͺ˵Ĵ�С*****//

    if (Owecolor == "red") {
        for (int i = 0; i < frame.rows * frame.cols; i++)
        {
            float* data = (float*)frame.data;
            //imgChannels[0].data[i] *= 0.75;
            imgChannels[0].data[i] *= 0.8;
        }
        cv::subtract(imgChannels[0], imgChannels[2], this->midImage);   //我方为红色，要识别蓝色
        //cv::subtract(imgChannels[0], imgChannels[1], this->midImage);   //我方为红色，要识别蓝色
    }
    else
    {
        for (int i = 0; i < frame.rows * frame.cols; i++)
        {
            float* data = (float*)frame.data;		//我方为蓝色，要识别红色
            //imgChannels[0].data[i] *= 0.75;
            imgChannels[2].data[i] *= 1;
        }
        cv::subtract(imgChannels[2], imgChannels[0], this->midImage);
    }
    //****************闭运算增强图通道相减后的图像**************//
    //***************用大的内核来加强处理的图像************//
    morphologyEx(this->midImage, this->midImage, MORPH_CLOSE, element1);  //闭运算

    cv::threshold(this->midImage, this->midImage, 75, 255, cv::THRESH_BINARY); //buff_blue阈值

    //cv::threshold(this->midImage, this->midImage, 50, 255, cv::THRESH_BINARY);  //blue4阈值
    //cv::add(src, this->midImage, this->final_image);
    //cv::addWeighted(src, 0.8, this->midImage, 0.4, 20, this->final_image);
    //imshow("midImage 2", midImage);

    cv::bitwise_and(src, midImage, this->final_image);
    //imshow("threshold 3", final_image);
    cv::threshold(this->final_image, this->final_image, 50, 255, cv::THRESH_BINARY);

    //Ptr<BackgroundSubtractor> ptrMOG2 = createBackgroundSubtractorMOG2();
    //ptrMOG2->apply(this->midImage, this->midImage);
    //bitwise_not(this->midImage, this->midImage);
    //floodFill(this->midImage, Point(0, 0), Scalar(0));
    //imshow("midImage", midImage);
    //****************��������ȥ��������**************//
    //morphologyEx(this->final_image, this->final_image, MORPH_OPEN, element4);  //开运算
    //cv::GaussianBlur(this->final_image, this->final_image, cv::Size(3, 3), 0); //去除高斯噪声

    morphologyEx(this->final_image, this->final_image, MORPH_CLOSE, element4);  //闭运算
    imshow("final_image open 2", final_image);

#if FloodFill
    //****************通过漫水填充能更好的去除背景的干扰噪声点**************//
		//****************通过膨胀和开运算来扩大填充后图像*********************//
		Mat foolFill_image;
		bitwise_not(this->final_image, foolFill_image);
		floodFill(foolFill_image, Point(0, 0), Scalar(0));
		imshow("floodFill 1", foolFill_image);
		morphologyEx(foolFill_image, foolFill_image, MORPH_CLOSE, element5);
		tool::ElementSize_dilate(foolFill_image, structElementSize);
		imshow("floodFill 01", foolFill_image);
		cv::bitwise_and(foolFill_image, final_image, this->final_image);
		imshow("floodFill 2", final_image);
#endif
}

//-------------保证ROI截图正确--------------------//
bool buffdistinguish::makeRectSafe(const Rect rect, const Size size) {
    if (rect.x < 0)
        return false;
    if (rect.x + rect.width > size.width)
        return false;
    if (rect.y < 0)
        return false;
    if (rect.y + rect.height > size.height)
        return false;
    if (rect.width <= 0 || rect.height <= 0)
        return false;
    return true;
}

//----------最小二乘法拟合圆------------//
bool buffdistinguish::LeastSquaresCircleFitting(vector<cv::Point2d> &m_Points)
{
    if (!m_Points.empty())
    {
        int iNum = (int)m_Points.size();
        if (iNum < 3)	return 1;
        double X1 = 0.0;
        double Y1 = 0.0;
        double X2 = 0.0;
        double Y2 = 0.0;
        double X3 = 0.0;
        double Y3 = 0.0;
        double X1Y1 = 0.0;
        double X1Y2 = 0.0;
        double X2Y1 = 0.0;
        vector<cv::Point2d>::iterator iter;
        vector<cv::Point2d>::iterator end = m_Points.end();
        for (iter = m_Points.begin(); iter != end; ++iter)
        {
            X1 = X1 + (*iter).x;
            Y1 = Y1 + (*iter).y;
            X2 = X2 + (*iter).x * (*iter).x;
            Y2 = Y2 + (*iter).y * (*iter).y;
            X3 = X3 + (*iter).x * (*iter).x * (*iter).x;
            Y3 = Y3 + (*iter).y * (*iter).y * (*iter).y;
            X1Y1 = X1Y1 + (*iter).x * (*iter).y;
            X1Y2 = X1Y2 + (*iter).x * (*iter).y * (*iter).y;
            X2Y1 = X2Y1 + (*iter).x * (*iter).x * (*iter).y;
        }
        double C = 0.0;
        double D = 0.0;
        double E = 0.0;
        double G = 0.0;
        double H = 0.0;
        double a = 0.0;
        double b = 0.0;
        double c = 0.0;
        C = iNum * X2 - X1 * X1;
        D = iNum * X1Y1 - X1 * Y1;
        E = iNum * X3 + iNum * X1Y2 - (X2 + Y2) * X1;
        G = iNum * Y2 - Y1 * Y1;
        H = iNum * X2Y1 + iNum * Y3 - (X2 + Y2) * Y1;
        a = (H * D - E * G) / (C * G - D * D);
        b = (H * C - E * D) / (D * D - G * C);
        c = -(a * X1 + b * Y1 + X2 + Y2) / iNum;
        double A = 0.0;
        double B = 0.0;
        double R = 0.0;
        A = a / (-2);
        B = b / (-2);
        R = double(sqrt(a * a + b * b - 4 * c) / 2);
        if (isnan(A) || isnan(B) || isinf(A) || isinf(B)|| isnan(R)|| isinf(R)) {
            return 0;
        }
        Centroid.x = A;
        Centroid.y = B;
        dRadius = R;
        return 0;
    }
    else
        return 1;
    return 0;
}

///----------------找到发光轮廓----------------//
bool buffdistinguish::findRect(const cv::Mat & frame_clone, vector<cv::Point2d> &m_Points)
{
    std::vector<std::vector<cv::Point>> contours;
    std::vector<cv::Vec4i> hierarchy;
    /******************************* 检测扇叶 *************************************/
    cv::findContours(this->final_image, contours, hierarchy,
                     cv::RETR_TREE, cv::CHAIN_APPROX_NONE, cv::Point());
    //-----------Ŀ��������Ӧ������Ϊ[-1, -1, -1, ������],��"ֻ�и�����"��һ����
    Buff fan_param;
    for (size_t i = 0; i < contours.size(); i++)
    {
        //***************不满足6点拟合椭圆************//
        if(contours[i].size() < 6){
            continue;
        }
        int sub = hierarchy[i][2];
        //cout << "sub " << sub << endl;
        //cout << "hierarchy[sub][2]_" << hierarchy[sub][2] << endl;*/
        //**************有子轮廓(parent)*************//
        if (sub != -1)
        {
            fan_param.area = contourArea(contours[i]);
            //cout << "fan_param.area" << fan_param.area << endl;

            //**************面积条件筛选****************//
            if (400 > fan_param.area&&fan_param.area > 2500) {
                continue;
            }
            fan_param.Rotation_rect = cv::minAreaRect(contours[i]);

            //**************长度条件筛选****************//
            float max_rrect_len = tool::max_rrect_len(fan_param.Rotation_rect);
            //cout << "max_rrect_len :" << max_rrect_len << endl;
            if ( max_rrect_len>200|| max_rrect_len<70) {
                continue;
            }

            //**************长宽比条件筛选****************//
            if (tool::lw_raio(fan_param.Rotation_rect) < 2.0f || tool::lw_raio(fan_param.Rotation_rect) > 4.0f) {
                continue;
            }

            //**************矩形周长与点集周长比值************//
            //所有扇叶基本在0.9~1.1之间
            fan_param.arclength_rate = fan_param.arclength / (fan_param.Rotation_rect.size.width * 2 + fan_param.Rotation_rect.size.height * 2);
            if (fan_param.arclength_rate > 1.3) {
                continue;
            }

            //**************矩形面积与点集面积比值************//
            //锤子扇叶基本为0.5  普通扇叶基本为0.9
            //基本找到锤子扇叶大轮廓
            fan_param.area_rate = fan_param.area / fan_param.Rotation_rect.size.area();
            //cout << "fan_param.area_rate" << fan_param.area_rate << endl;
            if (fan_param.area_rate > 0.5) {
                continue;
            }
            fan_param.is_big_fan = true;//找到大轮廓
            tool::drawRect(this->frame_clone,fan_param.Rotation_rect);

            fan_param.Rotation_rect.points(fan_param.vertices);
            float buff_0_1=tool::pointDistance(fan_param.vertices[0],fan_param.vertices[1]);
            float buff_1_2=tool::pointDistance(fan_param.vertices[1],fan_param.vertices[2]);
            if(buff_0_1>buff_1_2){
                fan_param.max_length_half=buff_0_1/2;
            }else if(buff_0_1>buff_1_2){
                fan_param.max_length_half=buff_1_2/2;
            }
            //***************寻找待击打的装甲板*************//下面条件都成立
            //if (hierarchy[sub][2] ==-1)     //上面找到锤子轮廓的基础上找到没有子轮廓的
            if (hierarchy[sub][0] == -1)      //没有next轮廓(即找到有相邻轮廓的) Previous轮廓不行
            {
                target_buff_param.area = contourArea(contours[sub]);//轮廓面积
                target_buff_param.Rotation_rect = minAreaRect(contours[sub]);//轮廓外接矩形
                if (target_buff_param.area < 400) {
                    continue;
                }
                float lw_raio = tool::lw_raio(target_buff_param.Rotation_rect);
                cout << "lw_raio" << lw_raio<<endl;
                if (lw_raio < 1.5) { continue; }
                target_buff_param.Rotation_rect.points(target_buff_param.vertices);
                target_buff_param.is_target_buff = true; //找到打击轮廓
                for (int i = 0; i < 4; i++)
                {
                    cv::line(this->frame_clone, target_buff_param.vertices[i], target_buff_param.vertices[(i + 1) % 4], cv::Scalar(0, 255, 0), 3);

                }
                cout<<"this->target_buff_param circle_center::11111111111111"<< target_buff_param.Rotation_rect.center<<endl;
                m_Points.push_back(target_buff_param.Rotation_rect.center);     //储存全部最小外接矩形中心点
                m_Points2 = target_buff_param.Rotation_rect.center;
                //cout << "m_Points" << m_Points.size() << endl;
                //LeastSquaresCircleFitting(m_Points);//拟合圆
                //cout << "Centroid x is:" << Centroid.x << endl;
                //cout << "Centroid y is:" << Centroid.y << endl;
                // if (isnan(Centroid.x) || isnan(Centroid.y) || isinf(Centroid.x) || isinf(Centroid.y)) {
                // 	continue;
                // }
                //cv::circle(this->frame_clone, this->Centroid, this->dRadius, Scalar(0, 0, 255), 2, 8);//����Բ

                cv::Point2f  center_armor_point1, center_armor_point2,new_center;
                cv::Size2f rotate_rect;
                double angle;
                if(target_buff_param.Rotation_rect.size.width> target_buff_param.Rotation_rect.size.height){
                    center_armor_point1.x = ((target_buff_param.vertices[0].x+ target_buff_param.vertices[1].x))/2;
                    center_armor_point1.y = ((target_buff_param.vertices[0].y + target_buff_param.vertices[1].y)) / 2;
                    center_armor_point2.x = ((target_buff_param.vertices[2].x + target_buff_param.vertices[3].x)) / 2;
                    center_armor_point2.y = ((target_buff_param.vertices[2].y + target_buff_param.vertices[3].y)) / 2;
                    rotate_rect.width = target_buff_param.Rotation_rect.size.width;
                    rotate_rect.height = dRadius;
                    angle = target_buff_param.Rotation_rect.angle;
                }
                else
                {
                    center_armor_point1.x = ((target_buff_param.vertices[1].x + target_buff_param.vertices[2].x)) / 2;
                    center_armor_point1.y = ((target_buff_param.vertices[1].y + target_buff_param.vertices[2].y)) / 2;
                    center_armor_point2.x = ((target_buff_param.vertices[0].x + target_buff_param.vertices[3].x)) / 2;
                    center_armor_point2.y = ((target_buff_param.vertices[0].y + target_buff_param.vertices[3].y)) / 2;
                    rotate_rect.width = dRadius;
                    rotate_rect.height = target_buff_param.Rotation_rect.size.height;
                    angle== target_buff_param.Rotation_rect.angle;
                }
                new_center.x = (((center_armor_point1.x + center_armor_point2.x) / 2+ Centroid.x)/2);
                new_center.y = (((center_armor_point1.y + center_armor_point2.y) / 2 + Centroid.y)/2);
                new_rotaterect = RotatedRect(new_center, rotate_rect, angle);
                Point2f new_rotate2[4];
                new_rotaterect.points(new_rotate2);
                //cv::line(this->frame_clone, center_armor_point1 , center_armor_point2, cv::Scalar(0, 255, 0), 2 );
                //imshow("frame_clone 2��", frame_clone);
            }
        }
            //**************有父轮廓(parent)*************//
        else
        {
            Circle_center _circle_param;
            _circle_param.circle_rect = cv::boundingRect(contours[i]);//
            _circle_param.circle_area = contourArea(contours[i]);//
            cout << "circle_param.circle_area::::::::" << _circle_param.circle_area << endl;
            if (_circle_param.circle_area < 60|| _circle_param.circle_area>650) { continue; }
            _circle_param.area_rate = _circle_param.circle_area / (_circle_param.circle_rect.width*_circle_param.circle_rect.height);

            tool::data(_circle_param.area_rate,"_circle_param.area_rate");

            //****************长宽比角度条件找圆心**************//可加入分类器
            this->circle_center =findCircleCenter(_circle_param, _circle_param.circle_area,fan_param);

#if _minEnclosingCircle
            //****************最小外接圆找出圆心***************//
					float radius,new_radius;
					cv::Point2f center;
					minEnclosingCircle(contours[i], center, radius);
					//if (radius > 10 || radius < 6) { continue; }
					//if (center.x==0 || center.y==0) { continue; }
					//min_center = center;
					//cv::circle(this->frame_clone, min_center, radius, Scalar(255, 0, 0), 4);
					//cout << "circle_param.circle_center rate :" << circle_param.circle_center.x / center.x << endl;
					//cout << "radius" << radius << endl;
				    //*****************收集点集拟合为一个圆******************//
					/*static  vector<Point> points;
					points.emplace_back(min_center);
					minEnclosingCircle(points, new_center, new_radius);*/
					//cv::circle(this->frame_clone, new_center, new_radius, Scalar(255, 0, 0), 4);
#endif

            if (this->circle_center.x == 0|| this->circle_center.y==0) { continue; }

            if (isnan(this->circle_center.x) || isnan(this->circle_center.y) || isinf(this->circle_center.x) || isinf(this->circle_center.y)) {
                return false;
            }
        }
    }

#if _minEnclosingCircle
    // if (isnan(dRadius2) || isnan(dRadius2) || isinf(dRadius2) || isinf(dRadius2)) {
	// 	continue;
	// }
	//cout << "circle_param.circle_center" << circle_param.circle_center.x << endl;
	//cout << "dRadius2" << dRadius1 << endl;
	// if (dRadius2>200|| dRadius2<90) {
	// 	continue;
	// }
	//if (min_center.x == 0 || min_center.y == 0) { continue; }
#endif
    if(possible_center.size()==0){return false; }

    if (target_buff_param.Rotation_rect.center.x == 0|| target_buff_param.Rotation_rect.center.y==0) { return false; }
    if(possible_center.size()>2){
        sort(possible_center.begin(),possible_center.end(),tool::campare_center_distance);
    }
    this->circle_center.x=possible_center[0].circle_rect.x+possible_center[0].circle_rect.width/2;
    this->circle_center.y=possible_center[0].circle_rect.y+possible_center[0].circle_rect.height/2;
    cout<<" this->circle_center::::::;;"<<  this->circle_center<<endl;
    //******************计算装甲板到圆心的距离****************//
    float dRadius1 = tool::pointDistance(this->circle_center, target_buff_param.Rotation_rect.center);
    cout<<"this->target_buff_param circle_center::::::;;"<< target_buff_param.Rotation_rect.center<<endl;
    //float dRadius2 = tool::pointDistance(min_center, target_buff_param.Rotation_rect.center);
    this->Centroid = this->circle_center;//圆心赋值
    this->dRadius = dRadius1;            //半径赋值
    cout << "dRadius1 :::::::::::" << dRadius1 << endl;
    //circle(this->frame_clone, this->circle_center, dRadius1, Scalar(0, 255, 0), 4);
    //circle(this->frame_clone, min_center, dRadius2, Scalar(0, 0, 255),2 );
    imshow("dwdw",frame_clone);
    //cv::drawContours(this->frame_clone, contours, -1, Scalar(255), 2);
}

//------------------得到图像ROI----------------------//
void buffdistinguish::getROI(Buff &fan_param) {
    RotatedRect new_fan= fan_param.Rotation_rect;
    new_fan.center = fan_param.Rotation_rect.center;
    new_fan.size.height = 2*tool::max_rrect_len(fan_param.Rotation_rect);
    new_fan.size.width = 2*tool::max_rrect_len(fan_param.Rotation_rect);
    Point2f vertices[4];
    new_fan.points(vertices);
    vector<Point2f> points;
    for (int i = 0; i < 4; i++)
    {
        points.push_back(vertices[i]);
    }
    roi_rect = boundingRect(points);
}


///-------------------传统方法找到大符圆心---------------   //传统方法长宽比直接找,可试着加上svm
cv::Point2f buffdistinguish::findCircleCenter(Circle_center &_circle_param, float area,Buff fan_param)
{
    //_circle_param.area_rate = _circle_param.circle_rect.size.area() / area;//contourArea==area
    //cout << "circle_param.area_rate" << circle_param.area_rate << endl;
    //cout << "_circle_param area_rate" << _circle_param.area_rate << endl;
    if (_circle_param.area_rate>0.6)
    {
        float lw_raio = tool::lw_rect_raio( _circle_param.circle_rect);
        ///cout << "circle_rect angle::::;" <<  _circle_param.circle_rect.angle << endl;
        if (lw_raio < 1.5)
        {
            //cout << "this->area :::" << _circle_param.circle_area << endl;
            if (_circle_param.circle_area < 500 && _circle_param.circle_area>60)
            {

                //cv::circle(this->frame_clone, _circle_param.circle_center , 2, Scalar(0, 0, 255), 6);
                _circle_param.circle_center.x=_circle_param.circle_rect.x+_circle_param.circle_rect.width/2;
                _circle_param.circle_center.y=_circle_param.circle_rect.y+_circle_param.circle_rect.height/2;
            }
        }
    }
    float center_distance = tool::pointDistance( _circle_param.circle_center, fan_param.Rotation_rect.center);
    if(center_distance>fan_param.max_length_half&&fan_param.max_length_half>0){
        _circle_param.distance_to_rect = center_distance;
    }
    this->possible_center.emplace_back(_circle_param);//�洢���п��ܵ�Բ��

    for (int i = 0; i < 4; i++)
    {
        cv::line(this->frame_clone, _circle_param.vertices[i], _circle_param.vertices[(i + 1) % 4], cv::Scalar(0, 255, 0), 4);

    }
    cv::circle(this->frame_clone, _circle_param.circle_center , 2, Scalar(0, 0, 255), 6);
    return  _circle_param.circle_center;
}

//---------------------得到大符的旋转方向-------------------//
int buffdistinguish::get_direction(float angle){
    static vector<float >angle_sum;
    static int counter_clockwise = 0;
    static int clockwise = 0;
    cout<< "angle 222 ::" << angle << endl;
    if (angle > 180||angle<-180) {
        return NULL ;
    }
    if (angle_sum.empty()) {
        angle_sum.emplace_back(angle);
    }
    else if(angle_sum.size() < 30)
    {
        if (fabs(angle - angle_sum[angle_sum.size() - 1]) < 20)
        {
            angle_sum.push_back(angle);
        }
        else
        {
            angle_sum.clear();
        }
        cout << "angle_sum.size()" << angle_sum.size() << endl;
        cout << "else if(angle_sum.size() < 30) " << endl;
        if (-180 < angle&&angle < 180) {
            angle_sum.emplace_back(angle);
        }
    }
    if (angle_sum.size() == 30)
    {
        if(angle_sum [30]-angle_sum[1]<5){
            int No_Rotate = 3;
            return No_Rotate;
        }
        for (size_t i = 0; i < 10; i++) {
            //cout << "angle_sum[i]" << angle_sum[i] << endl;
            if(angle_sum [30]-angle_sum[1]>5){
                if (angle_sum[i + 20] - angle_sum[i] < -3)
                {
                    clockwise++;
                }
                else if(angle_sum[i + 20] - angle_sum[i] >3)
                {
                    counter_clockwise++;
                }

                if (clockwise > counter_clockwise) {
                    int Clockwise = 1;
                    cout << "顺时针方向 1" << endl;
                    return Clockwise;
                }
                else
                {
                    int Counter_Clockwise = 2;
                    cout << "逆时针方向 1" << endl;
                    return Counter_Clockwise;
                }
            }
        }
        angle_sum.clear();
    }
}

void buffdistinguish::target_armor_height() {
    // 计算能量机关的高度

}

//----------用世界坐标点得到打符的绝对坐标----------//
void buffdistinguish::get_absoltion_point(double angle, bool x, bool y) {
    cv::Point3f absoltion_point;
    cv::Point3f circle_center_3d;	  //Բ��3d��
    double angle1 = fabs(angle);  //传入的角度都为正
    angle1 = tool::Radian(angle1);
    absoltion_point.z = Horizontal_Distance;
    if (x == 1) {  //对于水平x 二 三象限
        absoltion_point.x = circle_center_3d.x+ Radius * cos(angle1);
    }else{         //对于水平x 一 四象限
        absoltion_point.x = circle_center_3d.x- Radius * cos(angle1);
    }
    if (y == 1){  //对于竖直y 一 二象限
        absoltion_point.y = circle_center_3d.y+ Radius * sin(angle1);
    }else{		  //对于竖直y 三  四象限
        absoltion_point.y = circle_center_3d.y- Radius * sin(angle1);
    }
    tool::write_3Ddata(angle, absoltion_point);
    yaw_abs_angle = atan2(absoltion_point.x, absoltion_point.z);
    pit_abs_angle = atan2(absoltion_point.y, absoltion_point.z); //not PI/2
}

void buffdistinguish::get_gun_absoltion_point(float pit_angle, float yaw_angle)
{
    //通过圆心三维点使车对着圆心
    float angle;   //通过串口传入数据
    double pit_angle1=tool::Radian(pit_angle);
    double yaw_angle1 = tool::Radian(yaw_angle);
    cv::Point3f absoltion_point;
    absoltion_point.x = Horizontal_Distance * tan(yaw_angle1);
    absoltion_point.y = Horizontal_Distance * tan(pit_angle1);
    absoltion_point.z = Horizontal_Distance;
}

void buffdistinguish::prediect_buff(cv::RotatedRect &predict_Rect,cv::Point2f predict_newpoint,float predict_angle){
    predict_Rect = cv::RotatedRect(predict_newpoint, Size2f(25, 40), predict_angle);
    cout << "curRect l" << predict_Rect.center << endl;
    tool::drawRect(this->frame_clone, predict_Rect);
    cv::circle(this->frame_clone, predict_newpoint, 5, Scalar(0, 255, 0));
}

bool buffdistinguish::rotation_direction(cv::RotatedRect &predict_Rect) {
    const int frame_num = 20;
    static int time1 = 0;
    cv::Point2f c_coordinate;
    coordinate_x = this->m_Points2.x - this->Centroid.x;
    coordinate_y = this->m_Points2.y - this->Centroid.y;
    if (isnan(coordinate_x) || isnan(coordinate_y) || isinf(coordinate_x) || isinf(coordinate_y)|| coordinate_x ==0) {
        return 0;
    }

    //第一象限
    if (coordinate_x > 0 && coordinate_y < 0) {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第二象限
    else if (coordinate_x < 0 && coordinate_y < 0)
    {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第三象限
    else if (coordinate_x < 0 && coordinate_y > 0)
    {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
        //第四象限
    else {
        c_coordinate.x = coordinate_x;
        c_coordinate.y = -coordinate_y;
    }
    angle = atan2(c_coordinate.y, c_coordinate.x) * 180 / CV_PI;
    cout << "angle 11 :" << angle << endl;

    //***************得到扇叶旋转方向*************//
    int Clockwise=get_direction(angle);

    double rad =0;
    //Size2f size = box1.size();
    if (Clockwise==3) {
        cout << "static 静止状态 :"  << endl;
        return false;
    }
    //顺时针
    if (Clockwise==1) {
        cout << "顺时针方向 2" << endl;
        //在第一象限
        if (angle >= 0 && angle <= 90)
        {
            cout << "在第一象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            cout << "newangle :" << rad << endl;
            if (  -90<angle - advance_angle )
            {
                // 预测点在第一象限
                cout << "顺时针" << endl;
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                //cout << -(theta - PARAM) << endl;
                if(predict_newpoint.x== Centroid.x|| m_Points2.x==0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                get_absoltion_point(angle,0,1);  //   0-cos(60)*dRadius
                predict_angle = -(angle - advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);

            }
        }
        else if (angle > 90 && angle <= 180) //�ڵڶ�����
        {
            cout << "顺时针" << endl;
            cout << "在第二象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle >= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                angle = 180 - angle;    //�磺180-120=60 cos(60)*dRadius+0
                get_absoltion_point(angle,1,1);
                predict_angle = -(angle - advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }
        }
        else if (-180 < angle  && angle <= -90) //�ڵ�������
        {
            cout << "顺时针" << endl;
            cout << "在第三象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle <= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                angle =  - angle+180;      //�磺-120+180=60  cos(60)*dRadius+0
                get_absoltion_point(angle,1,0);//
                predict_angle = -(angle - advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }

        }
        else if (-90 < angle && angle <= 0) //�ڵ�������
        {
            cout << "顺时针" << endl;
            cout << "在第四象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle <= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                get_absoltion_point(angle,0,0);  //0 - cos(60)*dRadius
                predict_angle = -(angle - advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }
        }
    }
        //逆时针
    else if(Clockwise==2)
    {
        if (angle >= 0 && angle <= 90)
        {
            cout << "逆时针方向" << endl;
            cout << "在第一象限" << endl;
            if (angle - advance_angle >= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                cout << "predict_newpoint.x" << predict_newpoint.x << endl;
                cout << "predict_newpoint.x" << predict_newpoint.y << endl;
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                cout << "angle" << angle << endl;
                get_absoltion_point(angle,0,1);
                predict_angle = -(angle +advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }
        }
        else if (angle > 90 && angle <= 180)
        {
            cout << "逆时针方向" << endl;
            cout << "在第二象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle >= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                cout << "angle" << angle << endl;
                get_absoltion_point(angle,1,1);
                predict_angle = -(angle + advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);

            }
        }
        else if ((-180 < angle  && angle <= -90))//�ڵ�������
        {
            cout << "逆时针方向" << endl;
            cout << "在第三象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle <= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                cout << "angle" << angle << endl;
                get_absoltion_point(angle,1,0);
                predict_angle = -(angle+advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }
        }
        else if (-90 < angle && angle <= 0) //�ڵ�������
        {
            cout << "逆时针方向" << endl;
            cout << "在第四象限" << endl;
            rad = (angle - advance_angle) / 180. * 3.14;
            if (angle - advance_angle <= 0)
            {
                predict_newpoint.x = Centroid.x + dRadius * cos(rad);
                predict_newpoint.y = Centroid.y - dRadius * sin(rad);
                if (predict_newpoint.x == Centroid.x || m_Points2.x == 0) { return 0; }
                if (predict_newpoint.x < 50) { return 0; }
                cout << "angle" << angle << endl;
                get_absoltion_point(angle,0,0);
                predict_angle = -(angle +advance_angle);
                prediect_buff(predict_Rect,predict_newpoint, predict_angle);
            }
        }
    }
}

cv::Point2f buffdistinguish::calcPoint(Point2f center, double R, float angle)
{
    angle = angle / 180.0 * 3.1415926;
    return center + Point2f((float)cos(angle), (float)-sin(angle)) *(float)R;
}

void buffdistinguish::kalman_predict() {
    /** @brief 创建卡尔曼滤波器对象KF.
    @param dynamParams 2，状态向量的维度，二维列向量（角度，△角度）
    @param measureParams 1，测量向量的维度，一维列向量（角度）
    @param controlParams 0，控制向量的维度
     */
    cv::KalmanFilter KF(2, 1, 0);    //测出一个角度
    /* 定义三个列向量：状态向量Xk（state）、测量向量Zk（measurement）和控制向量Uk（control） */
    // 定义状态向量Xk（state）：不用它的值，所以不用初始化
    // 定义测量向量Zk（measurement）  //1个值角度
    // for循环中需要传入其测量值，所以初始化
    Mat measurement = Mat::zeros(1, 1, CV_32F);
    // 控制向量Uk（control）简单实例就不考虑了
    /* 初始化三个矩阵：状态转移矩阵Ak（transitionMatrix），控制矩阵Bk（controlMatrix）和测量矩阵Hk（measurementMatrix） */

    // 初始化状态转移矩阵Ak（transitionMatrix）：定值
    KF.transitionMatrix = (Mat_<float>(2, 2) << 1, 1, 0, 1);

    // 初始化测量矩阵Hk（measurementMatrix）：单位矩阵化
    setIdentity(KF.measurementMatrix);
    // 控制向量都没有，控制矩阵Bk（controlMatrix）更没有。
    /* 两个估计的状态向量：先验估计的状态向量（statePre）和后验估计的状态向量P(k-1|k-1)(statePost) */
    // 先验估计的状态向量（statePre）第一次循环完在函数predict中就自己初始化了
    // 初始化后验估计的状态向量P(statePost)：
    randn(KF.statePost, Scalar::all(0), Scalar::all(0.1));
    /* 定义两个噪声：系统噪声Wk  和   测量噪声Vk */
    // 定义系统噪声Wk：二维列向量
    Mat processNoise(2, 1, CV_32F);
    // 测量噪声Vk之后再说
    /* 两个噪声的协方差矩阵： 系统噪声(过程噪声)的协方差矩阵Qk（processNoiseCov）和测量噪声的协方差矩阵Rk（measurementNoiseCov）*/
    // 系统噪声(过程噪声)的协方差矩阵Qk（processNoiseCov）
    setIdentity(KF.processNoiseCov, Scalar::all(1e-5));
    //测量噪声的协方差矩阵Rk（measurementNoiseCov）
    setIdentity(KF.measurementNoiseCov, Scalar::all(1e-5));
    /** @brief 初始化系统噪声
    不加噪声的话就是匀速圆周运动，加了点噪声类似匀速圆周运动，因为噪声的原因，运动方向可能会改变
    */
    randn(processNoise, Scalar(0), Scalar::all(sqrt(KF.processNoiseCov.at<float>(0, 0))));
    /* 两个错误估计的协方差矩阵： 后验错误估计的协方差矩阵P（errorCovPost）和 errorCovPre 先验错误估计的协方差矩阵P*/
    // errorCovPost 后验错误估计的协方差矩阵P
    setIdentity(KF.errorCovPost, Scalar::all(1));
    // errorCovPre 先验错误估计的协方差矩阵P之后再说
    /* 圆周运动的设定 */
    // center图像中心点

    /* 上一次的点 */

    // 上一次的点的角度stateAngle：取状态向量state的第一个值，即角度
    double lastAngle = KF.statePost.at<float>(0);
    // 上一次的点的坐标statePt：（在圆周上）相对Mat画布的点。
    Point lastPt = calcPoint(this->Centroid, this->dRadius, angle);
    //Point lastPt(box1.center.x, box1.center.y);

    // 计算预测值
    KF.predict();

    /* 预测点 */

    // 预测点的角度predictAngle：取预测值prediction的第一个值，即角度
    double predictAngle = KF.statePre.at<float>(0);
    // 预测点的坐标predictPt：（在圆周上）相对Mat画布的点。
    Point predictPt = calcPoint(this->Centroid, this->dRadius, predictAngle);

    /* 测量点 */

    // 初始化测量向量Zk（measurement）:随机值；然后和上一次的测量值加起来
    // 表示每次的测量值都不一样，物体在移动
    measurement += Scalar(theRNG().uniform(10.0, 40.0));

    // 测量点的角度measAngle：取预测值measurement的第一个值，即角度
    double measAngle = measurement.at<float>(0);

    // 测量点的坐标measPt：（在圆周上）相对Mat画布的点。
    Point measPt = calcPoint(this->Centroid, this->dRadius, angle);

    // 融合更新
    KF.correct(measurement);

    // 最优点的角度measAngle：取预测值measurement的第一个值，即角度
    double optimalAngle = KF.statePost.at<float>(0);

    // 测量点的坐标measPt：（在圆周上）相对Mat画布的点。
    Point optimalPt = calcPoint(this->Centroid, this->dRadius, optimalAngle);
    /* 画 */
    // 轨道
    cv::circle(frame_clone, this->Centroid, this->dRadius, Scalar::all(70), 1);
    // 上一个的点（白色）
    tool::drawCross(frame_clone, lastPt, 3, Scalar(255, 255, 255));
    // 测量点（红色）
    tool::drawCross(frame_clone, measPt, 3, Scalar(0, 0, 255));
    // 预测点（绿色）
    tool::drawCross(frame_clone, predictPt, 10, Scalar(0, 255, 0));
    // 最优点（蓝色）
    tool::drawCross(frame_clone, optimalPt, 10, Scalar(255, 0, 0));
    // 测量点到预测点
    line(frame_clone, measPt, predictPt, Scalar(0, 255, 0), 2);
    // 测量点到最优点
    line(frame_clone, measPt, optimalPt, Scalar(255, 0, 0), 2);

    // cout << "角度:\n";
    // cout << lastAngle << endl;
    // cout << measAngle << endl;
    // cout << predictAngle << endl;
    // cout << optimalAngle << endl;
    imshow("kalman_predict_fram", frame_clone);
}

void buffdistinguish::dection(Target_Buff &_Buff,const cv::Mat &frame, int Mode, vector<cv::Point2d> &m_Points,string Owecolor) {
    double tt1 = getTickCount();
    this->possible_center.clear();

    ROI = frame.clone();
    //yolo_dection(frame, ROI);         //做ROI要clone()，原来的图片
    imagePreprocess(ROI, Owecolor);     //对视频进行预处理

    findRect(frame_clone, m_Points);

#if Predeict
    rotation_direction(_Buff.Rotation_rect);
	cv::Point2f vertices[4];
	_Buff.Rotation_rect.points(vertices);
	for(size_t i=0;i<4;i++){
	_Buff.big_armor_points.emplace_back(vertices[i]);
	}
#endif
    cv::Point2f vertices[4];
    target_buff_param.Rotation_rect.points(vertices);
    for(size_t i=0;i<4;i++){
        _Buff.big_armor_points.emplace_back(vertices[i]);
    }

    double tt2 = getTickCount();
    //1000 *总次数/一秒内重复的次数= 时间(ms)
    cout << "buffdistinguish all time is:::::::::" << ((tt2 - tt1) / getTickFrequency())* 1000  << endl;//时间(ms)
    //tool::data(((tt2 - tt1) / getTickFrequency())* 1000,"((tt2 - tt1) / getTickFrequency())* 1000");
    cv::imshow("videol", this->frame_clone);
}
